#include "stm32f4xx_hal.h"
#include "control.h"
//6 0.3
//单直立环直立不动参数 800 0.9

float Vertical_Kp=0,Vertical_Kd=0;
float Velocity_Kp=0,Velocity_Ki=0;

//float Vertical_Kp=-1217,Vertical_Kd=-1.5;
//float Velocity_Kp=11,Velocity_Ki=0.055;

//float Vertical_Kp=-1320,Vertical_Kd=-3.6;
//float Velocity_Kp=11,Velocity_Ki=0.055;
float Turn_Kp=0;

/***

ֱ直立环PD：Kp*Ek+Kd*EK_D
输入；期望角度 真实角度 真实角速度
出口；直立环输出

***/
int Vertical(float Med,float Angle,float gyro_Y)
{
	int PWM_out;
	
	PWM_out=Vertical_Kp*(Angle-Med)+Vertical_Kd*(gyro_Y-0);
	return PWM_out;
}

/***

ֱ速度环PI：Kp*Ek+Ki*Ek_S
出口；速度环输出

***/
int Velocity(int encoder_left,int encoder_right)
{
	static int PWM_out,Encoder_Err,Encoder_S,EnC_Err_Lowout,EnC_Err_Lowout_Last;
  float a=0.7;
	
	//计算速度偏差
	Encoder_Err=(encoder_left+encoder_right)-0;
	//对速度偏差进行低通滤波 low_out=(1-a)*Ek+a*low_out_last
	EnC_Err_Lowout=(1-a)*Encoder_Err+a*EnC_Err_Lowout_Last;
	EnC_Err_Lowout_Last=EnC_Err_Lowout;
	//对速度偏差积分，积分出位移
	Encoder_S+=EnC_Err_Lowout;
	//积分限幅
	Encoder_S=Encoder_S>10000?10000:(Encoder_S<(-10000)?(-10000):Encoder_S);
	//输出
	PWM_out=Velocity_Kp*EnC_Err_Lowout+Velocity_Ki*Encoder_S;
	
	return PWM_out;
}

/***

ֱ转向环：系数*Z轴角速度
出口；转向环输出

***/
int Turn(int gyro_Z)
{
	int PWM_out;
	
	PWM_out=Turn_Kp*gyro_Z;
	return PWM_out;
}
